/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.com.manchini.stereocv.utils;

import br.com.manchini.stereocv.bd.ControleBD;
import br.com.manchini.stereocv.classes.amostradisparidade.AmostraDisparidade;
import br.com.manchini.stereocv.classes.parametrosdisparidade.ParamentrosDisparidade;
import com.googlecode.javacv.cpp.opencv_core;
import static com.googlecode.javacv.cpp.opencv_calib3d.*;
import static com.googlecode.javacv.cpp.opencv_core.*;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import static com.googlecode.javacv.cpp.opencv_imgproc.*;
import java.awt.Color;
import java.util.ArrayList;

/**
 *
 * @author merito
 */
public class ImgUtils {

    /**
     * Variaveis para Rotacioanr a Imagem
     */
    private static opencv_core.CvPoint2D32f my_center;
    static final int flags = CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS;
    static final opencv_core.CvScalar fillval = cvScalarAll(0);
    static final opencv_core.CvMat map_matrix = cvCreateMat(2, 3, CV_32FC1);

    /**
     *
     */
    /**
     * Metodo para rotacionar a imagem para o lado 90 graus
     *
     * @param imgOrigem
     * @param imgDest
     */
    public static void rotacionaImagem(opencv_core.IplImage imgOrigem, opencv_core.IplImage imgDest) {


        if (my_center == null) {
            my_center = new opencv_core.CvPoint2D32f(imgOrigem.asCvMat().cols() / 2,
                    imgOrigem.asCvMat().rows()/ 2);
            //DEFINA A ROTACAO DE 90 graus
            cv2DRotationMatrix(my_center, 90.0, 1.0, map_matrix);
        }
        cvWarpAffine(imgOrigem, imgDest, map_matrix, flags,
                fillval);
    }

    public static IplImage procurarPintaTabuleiro(opencv_core.IplImage img, Integer tabuleiroX, Integer tabuleiroY) {

        final opencv_core.CvSize boardSize = new opencv_core.CvSize(tabuleiroX, tabuleiroY);
        final opencv_core.CvPoint2D32f imageCorners = new opencv_core.CvPoint2D32f(boardSize.width() * boardSize.height());


        int achouEsq = cvFindChessboardCorners(img, boardSize, imageCorners, null, CV_CALIB_CB_ADAPTIVE_THRESH);
        if (achouEsq == 1) {
            opencv_core.CvMat imageDst = opencv_core.CvMat.create(img.height(), img.width(), CV_8UC2, 1);
            cvCvtColor(img, imageDst, CV_RGB2GRAY);
            cvFindCornerSubPix(imageDst, imageCorners, 0, new CvSize(11, 11), new CvSize(-1, -1), new CvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
            cvDrawChessboardCorners(imageDst, boardSize, imageCorners, imageCorners.sizeof(), achouEsq);

            return imageDst.asIplImage();
        } else {
            return img;
        }
    }

    public static void desenhaMiraImagem(IplImage imgFinal) {
        int x = 0;
        while (x < imgFinal.width()) {
            cvLine(imgFinal, new CvPoint(x, 0), new CvPoint(x, imgFinal.height()), CvScalar.GREEN, 0, 0, 0);
            x += 30;
        }
        int y = 0;
        while (y < imgFinal.height()) {
            cvLine(imgFinal, new CvPoint(0, y), new CvPoint(imgFinal.width(), y), CvScalar.RED, 0, 0, 0);
            y += 30;
        }


    }

    public static double estimarDistancia(Color coleta, int idParametros) {
        ControleBD controleBD = new ControleBD();
        double retorno = 0.0;
        
        try {

            ParamentrosDisparidade paramentrosDisparidade = new ParamentrosDisparidade();
            paramentrosDisparidade.setId(idParametros);
            if ((paramentrosDisparidade = (ParamentrosDisparidade) controleBD.existe(paramentrosDisparidade)) != null) {
                //Encontrei um parametro
                ArrayList<AmostraDisparidade> amostras = paramentrosDisparidade.getAmostras();
                
                //Amostra de Menor e Maior Disparidade, mais proxima da coleta
                AmostraDisparidade amostraMenor = amostras.get(amostras.size()-1);
                AmostraDisparidade amostraMaior = amostras.get(0);
                
                for (int i = 0; i < amostras.size(); i++) {
                    AmostraDisparidade amostra = amostras.get(i);
                    
                    //Pega como base o AZUL
                    //DISPARIDADE é inversa a distancia
                    
                    /**Busca a mostra de menor disparidade
                     * SE a disparidade atual for menor ou igual a menor
                     * E ainda for menor que a coleta
                    */
                    if(amostra.getColor().getBlue()>=amostraMenor.getColor().getBlue()
                            && amostra.getColor().getBlue()<=coleta.getBlue()){
                        amostraMenor = amostra;                        
                    }
                    /**
                     * Busca a maior amostra mais proxima
                     * SE a disparidade atual for maior ou igual a maior 
                     * e AINDA FOR MAIOR que a coleta
                     */
                    if (amostra.getColor().getBlue() <= amostraMaior.getColor().getBlue()
                            && amostra.getColor().getBlue() >= coleta.getBlue()) {
                        amostraMaior = amostra;                      
                    }
                }
                    //SE as 2 amostras forem iguais
                    //tem a distancia exata
                    if(amostraMaior.getColor().getBlue()==amostraMenor.getColor().getBlue()){
                     retorno = amostraMaior.getDistancia();
                    }else{
                        //REGRA DE 3
                        retorno = ((amostraMaior.getDistancia()+amostraMenor.getDistancia())*coleta.getBlue())/(amostraMaior.getColor().getBlue()+amostraMenor.getColor().getBlue());
                        
                    }
                    
                    
                    
                
                
            }
            
            
            
        } catch (Exception e) {
        } finally {
            controleBD.close();

        }
        return retorno;

    }
    
    

    
//
//    public static void stereoCalib(ArrayList<ParImagens> imagens, int nx, int ny, int useUncalibrated, float _squareSize) {
//        int displayCorners = 1;
//        int showUndistorted = 1;
//        boolean isVerticalStereo = false; //OpenCV can handle left-right
//        //or up-down camera arrangements
//        int maxScale = 1;
//        float squareSize = _squareSize; //Chessboard square size in cm
//        int i, j, lr, nframes, n = nx * ny, N = 0;
//        String[] imageNames = new String[2];
//        CvPoint3D32f[] objectPoints;
//        CvPoint2D32f[] points = new CvPoint2D32f[2];
//        int[] npoints;
//        char[] active = new char[2];
//        CvPoint2D32f[] temp = new CvPoint2D32f[n];
//        CvSize imageSize = new CvSize(0, 0);
//        // ARRAY AND VECTOR STORAGE:
//        double[][] M1 = new double[3][3];
//        double[][] M2 = new double[3][3];
//        double[] D1 = new double[5];
//        double[] D2 = new double[5];
//        double[][] R = new double[3][3];
//        double[] T = new double[3];
//        double[][] E = new double[3][3];
//        double[][] F = new double[3][3];
//        double[][] Q = new double[4][4];
//        CvMat _M1 = CvMat.create(3, 3, CV_64F);
//        CvMat _M2 = CvMat.create(3, 3, CV_64F);
//        CvMat _D1 = CvMat.create(1, 5, CV_64F);
//        CvMat _D2 = CvMat.create(1, 5, CV_64F);
//        CvMat _R = CvMat.create(3, 3, CV_64F);
//        CvMat _T = CvMat.create(3, 1, CV_64F);
//        CvMat _E = CvMat.create(3, 3, CV_64F);
//        CvMat _F = CvMat.create(3, 3, CV_64F);
//        CvMat _Q = CvMat.create(4, 4, CV_64F);
//        CanvasFrame canvas;
//        if (displayCorners == 1) {
////        cvNamedWindow("corners", 1);
//            canvas = new CanvasFrame("corners");
//        }
//        // READ IN THE LIST OF CHESSBOARDS:
//
//        for (int k = 0; k < imagens.size(); k++) {
//            ParImagens parImagens = imagens.get(k);
//            IplImage img = cvLoadImage(parImagens.getImgDir().toString());
//            imageSize = cvGetSize(img);
//            imageNames[0] = parImagens.getImgDir().getPath();
//            for (int s = 1; s <= maxScale; s++) {
//                IplImage timg = img;
//                if (s > 1) {
//                    timg = cvCreateImage(cvSize(img.width(), img.height()),
//                            img.depth(), img.nChannels());
//                    cvResize(img, timg, CV_INTER_CUBIC);
//                }
//                int result = cvFindChessboardCorners(timg, cvSize(nx, ny),
//                        temp[0],
//                        new int[]{CV_CALIB_CB_ADAPTIVE_THRESH
//                    | CV_CALIB_CB_NORMALIZE_IMAGE}, 1);
//                if (timg != img) {
//                    cvReleaseImage(timg);
//                }
//
//                if (result == 1) {
//                    break;
//                }
//            }
//
//        }
//
//        for (i = 0;; i++) {
//            char buf[
//            
//            1024];
//        int count = 0, result = 0;
//            lr = i % 2;
//            vector<CvPoint2D32f>
//            & pts = points[lr];
//            if (!fgets(buf, sizeof(buf) - 3, f)) {
//                break;
//            }
//            size_t len = strlen(buf);
//            while (len > 0 && isspace(buf[len - 1])) {
//                buf[--len] = '\0';
//            }
//            if (buf[0] == '#') {
//                continue;
//            }
//            IplImage * img = cvLoadImage(buf, 0);
//            if (!img) {
//                break;
//            }
//            imageSize = cvGetSize(img);
//            imageNames[lr].push_back(buf);
//            //FIND CHESSBOARDS AND CORNERS THEREIN:
//            for (int s = 1; s <= maxScale; s++) {
//                IplImage * timg = img;
//                if (s > 1) {
//                    timg = cvCreateImage(cvSize(img - > width * s, img - > height * s),
//                            img - > depth, img - > nChannels);
//                    cvResize(img, timg, CV_INTER_CUBIC);
//                }
//                result = cvFindChessboardCorners(timg, cvSize(nx, ny),
//                         & temp[0],  & count,
//                        CV_CALIB_CB_ADAPTIVE_THRESH
//                        | CV_CALIB_CB_NORMALIZE_IMAGE);
//                if (timg != img) {
//                    cvReleaseImage( & timg);
//                }
//                if (result || s == maxScale) {
//                    for (j = 0; j < count; j++) {
//                        temp[j].x /= s;
//                        temp[j].y /= s;
//                    }
//                }
//                if (result) {
//                    break;
//                }
//            }
//            if (displayCorners) {
//                printf("%s\n", buf);
//                IplImage * cimg = cvCreateImage(imageSize, 8, 3);
//                cvCvtColor(img, cimg, CV_GRAY2BGR);
//                cvDrawChessboardCorners(cimg, cvSize(nx, ny),  & temp[0],
//                        count, result);
//                cvShowImage("corners", cimg);
//                cvReleaseImage( & cimg);
//                if (cvWaitKey(0) == 27) //Allow ESC to quit
//                {
//                    exit(-1);
//                }
//            } else {
//                putchar('.');
//            }
//            N = pts.size();
//            pts.resize(N + n, cvPoint2D32f(0, 0));
//            active[lr].push_back((uchar) result);
//            //assert( result != 0 );
//            if (result) {
//                //Calibration will suffer without subpixel interpolation
//                cvFindCornerSubPix(img,  & temp[0], count,
//                        cvSize(11, 11), cvSize(-1, -1),
//                        cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS,
//                        30, 0.01));
//                copy(temp.begin(), temp.end(), pts.begin() + N);
//            }
//            cvReleaseImage( & img);
//        }
//        fclose(f);
//        printf("\n");
//        // HARVEST CHESSBOARD 3D OBJECT POINT LIST:
//        nframes = active[0].size(); //Number of good chessboads found
//        objectPoints.resize(nframes * n);
//        for (i = 0; i < ny; i++) {
//            for (j = 0; j < nx; j++) {
//                objectPoints[i * nx + j] = cvPoint3D32f(i * squareSize, j * squareSize, 0);
//            }
//        }
//        for (i = 1; i < nframes; i++) {
//            copy(objectPoints.begin(), objectPoints.begin() + n,
//                    objectPoints.begin() + i * n);
//        }
//        npoints.resize(nframes, n);
//        N = nframes * n;
//        CvMat _objectPoints = cvMat(1, N, CV_32FC3,  & objectPoints[0]);
//        CvMat _imagePoints1 = cvMat(1, N, CV_32FC2,  & points[0][0]);
//        CvMat _imagePoints2 = cvMat(1, N, CV_32FC2,  & points[1][0]);
//        CvMat _npoints = cvMat(1, npoints.size(), CV_32S,  & npoints[0]);
//        cvSetIdentity( & _M1);
//        cvSetIdentity( & _M2);
//        cvZero( & _D1);
//        cvZero( & _D2);
//
//        // CALIBRATE THE STEREO CAMERAS
//
//        
//        cvStereoCalibrate(  _objectPoints,   _imagePoints1,
//                  _imagePoints2,   _npoints,
//                  _M1,   _D1,   _M2,   _D2,
//                imageSize,  _R,   _T,   _E,   _F,
//                cvTermCriteria(CV_TERMCRIT_ITER
//                + CV_TERMCRIT_EPS, 100, 1e-5),
//                CV_CALIB_FIX_ASPECT_RATIO
//                + CV_CALIB_ZERO_TANGENT_DIST
//                + CV_CALIB_SAME_FOCAL_LENGTH);
//
//        // CALIBRATION QUALITY CHECK
//        // because the output fundamental matrix implicitly
//        // includes all the output information,
//        // we can check the quality of calibration using the
//        // epipolar geometry constraint: m2^t*F*m1=0
//        CvPoint3D32f[] lines = new CvPoint3D32f[2];
//    points[0].resize(N);
//        points[1].resize(N);
//        _imagePoints1 = cvMat(1, N, CV_32FC2,  & points[0][0]);
//        _imagePoints2 = cvMat(1, N, CV_32FC2,  & points[1][0]);
//        lines[0].resize(N);
//        lines[1].resize(N);
//        CvMat _L1 = cvMat(1, N, CV_32FC3,  & lines[0][0]);
//        CvMat _L2 = cvMat(1, N, CV_32FC3,  & lines[1][0]);
//        //Always work in undistorted space
//        cvUndistortPoints( & _imagePoints1,  & _imagePoints1,
//                 & _M1,  & _D1, 0,  & _M1);
//        cvUndistortPoints( & _imagePoints2,  & _imagePoints2,
//                 & _M2,  & _D2, 0,  & _M2);
//        cvComputeCorrespondEpilines( & _imagePoints1, 1,  & _F,  & _L1);
//        cvComputeCorrespondEpilines( & _imagePoints2, 2,  & _F,  & _L2);
//        double avgErr = 0;
//        for (i = 0; i < N; i++) {
//            double err = fabs(points[0][i].x * lines[1][i].x
//                    + points[0][i].y * lines[1][i].y + lines[1][i].z)
//                    + fabs(points[1][i].x * lines[0][i].x
//                    + points[1][i].y * lines[0][i].y + lines[0][i].z);
//            avgErr += err;
//        }
//        printf("avg err = %g\n", avgErr / (nframes * n));
//        //COMPUTE AND DISPLAY RECTIFICATION
//        if (showUndistorted) {
//            CvMat * mx1 = cvCreateMat(imageSize.height,
//                    imageSize.width, CV_32F);
//            CvMat * my1 = cvCreateMat(imageSize.height,
//                    imageSize.width, CV_32F);
//            CvMat * mx2 = cvCreateMat(imageSize.height,
//                    imageSize.width, CV_32F);
//            CvMat * my2 = cvCreateMat(imageSize.height,
//                    imageSize.width, CV_32F);
//            CvMat * img1r = cvCreateMat(imageSize.height,
//                    imageSize.width, CV_8U);
//            CvMat * img2r = cvCreateMat(imageSize.height,
//                    imageSize.width, CV_8U);
//            CvMat * disp = cvCreateMat(imageSize.height,
//                    imageSize.width, CV_16S);
//            CvMat * vdisp = cvCreateMat(imageSize.height,
//                    imageSize.width, CV_8U);
//            CvMat * pair;
//            double R1[
//            
//            3][3], R2[3][3]
//            , P1[3][4]
//            , P2[3][4];
//            CvMat _R1 = cvMat(3, 3, CV_64F, R1);
//            CvMat _R2 = cvMat(3, 3, CV_64F, R2);
//            // IF BY CALIBRATED (BOUGUET'S METHOD)
//            if (useUncalibrated == 0) {
//                CvMat _P1 = cvMat(3, 4, CV_64F, P1);
//                CvMat _P2 = cvMat(3, 4, CV_64F, P2);
//                cvStereoRectify( & _M1,  & _M2,  & _D1,  & _D2, imageSize,
//                         & _R,  & _T,
//                         & _R1,  & _R2,  & _P1,  & _P2,  & _Q,
//                        0/*CV_CALIB_ZERO_DISPARITY*/);
//                isVerticalStereo = fabs(P2[1][3]) > fabs(P2[0][3]);
//                //Precompute maps for cvRemap()
//                cvInitUndistortRectifyMap( & _M1,  & _D1,  & _R1,  & _P1, mx1, my1);
//                cvInitUndistortRectifyMap( & _M2,  & _D2,  & _R2,  & _P2, mx2, my2);
//
//                //Save parameters
//                cvSave("M1.xml",  & _M1);
//                cvSave("D1.xml",  & _D1);
//                cvSave("R1.xml",  & _R1);
//                cvSave("P1.xml",  & _P1);
//                cvSave("M2.xml",  & _M2);
//                cvSave("D2.xml",  & _D2);
//                cvSave("R2.xml",  & _R2);
//                cvSave("P2.xml",  & _P2);
//                cvSave("Q.xml",  & _Q);
//                cvSave("mx1.xml", mx1);
//                cvSave("my1.xml", my1);
//                cvSave("mx2.xml", mx2);
//                cvSave("my2.xml", my2);
//
//            } //OR ELSE HARTLEY'S METHOD
//            else if (useUncalibrated == 1 || useUncalibrated == 2) // use intrinsic parameters of each camera, but
//            // compute the rectification transformation directly
//            // from the fundamental matrix
//            {
//                double H1[
//                
//                3][3], H2[3][3]
//                , iM[3][3];
//                CvMat _H1 = cvMat(3, 3, CV_64F, H1);
//                CvMat _H2 = cvMat(3, 3, CV_64F, H2);
//                CvMat _iM = cvMat(3, 3, CV_64F, iM);
//                //Just to show you could have independently used F
//                if (useUncalibrated == 2) {
//                    cvFindFundamentalMat( & _imagePoints1,
//                             & _imagePoints2,  & _F);
//                }
//                cvStereoRectifyUncalibrated( & _imagePoints1,
//                         & _imagePoints2,  & _F,
//                        imageSize,
//                         & _H1,  & _H2, 3);
//                cvInvert( & _M1,  & _iM);
//                cvMatMul( & _H1,  & _M1,  & _R1);
//                cvMatMul( & _iM,  & _R1,  & _R1);
//                cvInvert( & _M2,  & _iM);
//                cvMatMul( & _H2,  & _M2,  & _R2);
//                cvMatMul( & _iM,  & _R2,  & _R2);
//                //Precompute map for cvRemap()
//                cvInitUndistortRectifyMap( & _M1,  & _D1,  & _R1,  & _M1, mx1, my1);
//
//                cvInitUndistortRectifyMap( & _M2,  & _D1,  & _R2,  & _M2, mx2, my2);
//            } else {
//                assert (0);
//            }
//            cvNamedWindow("rectified", 1);
//            // RECTIFY THE IMAGES AND FIND DISPARITY MAPS
//            if (!isVerticalStereo) {
//                pair = cvCreateMat(imageSize.height, imageSize.width * 2,
//                        CV_8UC3);
//            } else {
//                pair = cvCreateMat(imageSize.height * 2, imageSize.width,
//                        CV_8UC3);
//            }
//            //Setup for finding stereo corrrespondences
//            CvStereoBMState * BMState = cvCreateStereoBMState();
//            assert (BMState != 0);
//            BMState - > preFilterSize = 41;
//            BMState - > preFilterCap = 31;
//            BMState - > SADWindowSize = 41;
//            BMState - > minDisparity = -64;
//            BMState - > numberOfDisparities = 128;
//            BMState - > textureThreshold = 10;
//            BMState - > uniquenessRatio = 15;
//            for (i = 0; i < nframes; i++) {
//                IplImage * img1 = cvLoadImage(imageNames[0][i].c_str(), 0);
//                IplImage * img2 = cvLoadImage(imageNames[1][i].c_str(), 0);
//                if (img1 && img2) {
//                    CvMat part;
//                    cvRemap(img1, img1r, mx1, my1);
//                    cvRemap(img2, img2r, mx2, my2);
//                    if (!isVerticalStereo || useUncalibrated != 0) {
//                        // When the stereo camera is oriented vertically,
//                        // useUncalibrated==0 does not transpose the
//                        // image, so the epipolar lines in the rectified
//                        // images are vertical. Stereo correspondence
//                        // function does not support such a case.
//                        cvFindStereoCorrespondenceBM(img1r, img2r, disp,
//                                BMState);
//                        cvNormalize(disp, vdisp, 0, 256, CV_MINMAX);
//                        cvNamedWindow("disparity");
//                        cvShowImage("disparity", vdisp);
//                    }
//                    if (!isVerticalStereo) {
//                        cvGetCols(pair,  & part, 0, imageSize.width);
//                        cvCvtColor(img1r,  & part, CV_GRAY2BGR);
//                        cvGetCols(pair,  & part, imageSize.width,
//                                imageSize.width * 2);
//                        cvCvtColor(img2r,  & part, CV_GRAY2BGR);
//                        for (j = 0; j < imageSize.height; j += 16) {
//                            cvLine(pair, cvPoint(0, j),
//                                    cvPoint(imageSize.width * 2, j),
//                                    CV_RGB(0, 255, 0));
//                        }
//                    } else {
//                        cvGetRows(pair,  & part, 0, imageSize.height);
//                        cvCvtColor(img1r,  & part, CV_GRAY2BGR);
//                        cvGetRows(pair,  & part, imageSize.height,
//                                imageSize.height * 2);
//                        cvCvtColor(img2r,  & part, CV_GRAY2BGR);
//                        for (j = 0; j < imageSize.width; j += 16) {
//                            cvLine(pair, cvPoint(j, 0),
//                                    cvPoint(j, imageSize.height * 2),
//                                    CV_RGB(0, 255, 0));
//                        }
//                    }
//                    cvShowImage("rectified", pair);
//                    if (cvWaitKey() == 27) {
//                        break;
//                    }
//                }
//                cvReleaseImage( & img1);
//                cvReleaseImage( & img2);
//            }
//            cvReleaseStereoBMState( & BMState);
//            cvReleaseMat( & mx1);
//            cvReleaseMat( & my1);
//            cvReleaseMat( & mx2);
//            cvReleaseMat( & my2);
//            cvReleaseMat( & img1r);
//            cvReleaseMat( & img2r);
//            cvReleaseMat( & disp);
//        }
//    }
//
//
}
